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ABSTRACT 






The performance of a modified linear Kalman filter with 
adaptation is compared with that of a common adaptive 
alpha-beta filter for state estimation of a pilot 

ccntrolled, ground directed bombing system. Of particular 
concern is the accuracy and response of the alternarive 
filters when the aircraft conducts random maneuvers in the 
vicinity of the target. The desirablity of including 
deterministic forcing in the filter model is discussed and a 
technique utilizing an adaptive Kalman identifier to 
establish the pilot response to ground control heading 
commands is presented. 
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I. INTRODOCTION 



The 


ground 


dire cted 


bombing 


system s 


imula ted 


is 


conceptually si 


milar to 


the 


OSMC 


AN/TPB- ID 


produced 


by 


Sierra 


Research 


Corporation. 


This system 


tracks 


the 



tactical aircraft with a conical scan radar, filters the 
noisy radar data, calculates heading commands based on the 
smoothed trajectory, and transmits this guidance information 
to the pilot via the Tacan navigation system located in the 
cockpit. This heading information directs the pilot to fly 
the aircraft so that its ground track vector passes through 
the calculated ordnance release point. Audio signals 
transmitted to the pilot designate the bomb release time. 

In an operational environment such a system would 
possibly be required to track and guide aircraft conducting 



significant maneuvers enroute 


to the target. 


T hese 


maneuvers would 


most likely be d 


ictated by tactical 


doct rine 


or by the threat 


environment. 






With this 


operational mod 


el in mind an app 


ropr iate 


concern is the 


capability of 


a ground directed 


bombing 


system to track 


and guide an 


aircraft exhibiting 


random 


maneuvers until 


moments prior 


to bomb release. 


It is 


obvious that the 


smoothing filts 


r should be able to 


res pond 
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to maneuvers, yet settle quickly to an accurate solution as 
the pilot steadies the aircraft. These conflicting 

requirements are investigated utilizing both alpha-beta and 
Kalman filtering techniques. 

Similar filtering techniques were utilized for a 
simulation of the US MC AN/TPQ-27, [1], However in that 
ground directed bombing system, control signals were 
directly coupled to the aircraft aerodynamic controls, thus 
eliminating the uncertainty of pilot response in the control 
loop. In that study significant improvements in filter 



response 


and 


accuracy 


were 


realized 


by including 


determinist 


ic 


forcing au 


topilo t 


commands 


in the state 


estimation 


via 


the Kalman 


filters. 
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II. GROUND DIRSCTBC BOMBING SYSTEM rGDBS) SIMULATION 



A. COORDINATE SYSTEMS 

A Cartesian coordinate system was chosen for the 
aircraft dynamics model and a radar centered inertial 
reference frame. In this reference system the y-axis is 
directed toward true North and the x-axis toward the east. 
The 2-axis is directed away from the center of the earth. 
All radar measurements of aircraft position, however, are 
obtained in spherical polar coordinates, i.e. slant range 
(R) , azimith angle from true North (A) , and elevation angle 
(E) from the horizontal. Figure 2,1 shows these coordinate 
systems and their transformation relationship. Wind is 
modeled with a constant velocity in the x-y plane with no 
vertical component. 

Curvature of the earth and the fact that pilot heading 
information is oriented to magnetic north, were not taken 
into account in the simulation. Also bomb ballistics and 
therefore coriolis forces were not included in the model. 
The aircraft is simply directed to a release point in space, 
which in a full simulation would be derived from the 
projected bomb trajectory , ballistic winds, coriolis forces, 
and a number of other factors, all of which are important to 




?ig. 2.1. Model Simulation Coordinate Systems 



12 



the problem as a whole but are not necessarily germane to 
the objective of evaluating the response and accuracy of 
alternative state estimating filters for a goal oriented 
maneuvering bomber. Thus the simulation has been simplified 
appropriately. 



B. AIRCRAFT DYNAMICS MODEL 

The dynamic model of the aircraft for the purpose of 
simulation was assumed to be a free inertial (1/s^) plant 
since the bombing profile dictates a constant aircraft 
airspeed. The discrete realization of this plant is shown 
in (2. 1) . 



X(k + 1) = 0(k+1/k)X(k) + A{k+1/k)U(k) 



where 



0(k+1A) 



1 T 0 0 0 0 
0 1 0 0 0 0 
0 0 1 T 0 0 
0 0 0 1 0 0 
0 0 0 0 1 T 
0 0 0 0 0 1 



( 2 . 1 ) 



( 2 . 2 ) 
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A(k+1A) 



T /^2 0 0 

TOO 

0 t5'2 0 

0 T 0 

0 0 T/'2 

0 0 T 



(2.3) 



In the controlled mode rhe aircraft model simulates the 
pilot responding to heading inputs transmitted from the 
radar site to the TAG AN navigation system in the cockpit. 
The model is driven by a pilot/aircraft control function 
similar to that developed in [ 1 ] and shown in Figure 2.2. 

The input is bearing to the target and the output is a bank 
angle which generates a heading rate that can be transformed 
into x-y acceleration components for entry into the dynamic 
model. It is assumed that in the controlled mode heading 
changes are made with coordinated turns performed by the 
pilot in response to heading commands displayed by the 
TACAN. The pilot /aircraft controller induced x-y 

accelerations are depicted in Figure 2.3 and summarized by 
(2.4) and (2.5) below. 

x(k) = V(k) cos(i//(k) ) i^(k) (2.4) 



y{k) =-7 (k) sin(i//(k) ) t^(k) 



(2.5) 
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Controller 




Fig. 2.2. Pilot/ Aircraft Controller Configuration 



i/>(k) and 7 (k) are aircraft heading and velocity respectively 
at time k, and is the heading rate which is derived in 

(2.6) through (2.9) from the free-body diagram shown in 
Figure 2.4. The aircraft weight is shown in (2.6) below. 

W = mg = L COS& (2.6) 
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Fig. 2.3. Pilot Induced X-Y Accelerations 

Equation (2.7) depicts the centripetal force generated in a 
turn, where S is the turn radius, L is lift, V is velocity, 
and d is the bank angle. 

? = aiV/'R = L sin (9 (2.7) 

Eut V/H si/' ,the curn rate, so 

P = jiViA = L sin d (2.8) 
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L sind 




Pig. 2.4. Coordinated Turn Free-Body Diagram 

Dividing (2.8) by (2.6) and rearranging terms yields (2.9), 
which defines ^ , the aircraft turn rate, as a function of 
aircraft bank angle and velocity V. 

^ = g/V tan 5 (2.9) 

From [1] the aircraft roll response is assumed to be of 
the form shown in (2.10) where T is the roll response time 
constant. 
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= i/(sr ♦ 1) 



( 2 . 10 ) 



e 

T 

No effort has been made to specifically model pilot delays 
or response to visual inputs from the TACAN. 

In the maneuvering mode the maneuver model described in 
[2] was used and is shown in Figures 2.5 and 2.6 . 




Fig. 2.5. Acceleration Probability Density Model 



This was simulated for uncontrolled random flight since the 
aircraft is assumed to typically move at a constant velocity 
with rums, evasive maneuvers, and air turbulence 
interpreted as perturbations upon rhe constant velocity 
trajectory. These maneuver perturbations or accelerations 
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Fig. 2.6. Hod el Acceleration Correlation Function 



can be specified by a magnitude, with probability P(a) from 
(2.11), and duration of S{a) from (2.12), the correlarion 
function of aircrafr acceleration. 

P(a) = (1-(2Pi + Pq) )/2A (2.11) 



R(a) = M exp (-t| a| ) 

The accelerarion A in (2.11) 
reasonably be expected From 
environment described. P^ is 
the maximum acceleration ±A, P^ 



( 2 . 12 ) 

is the maximum that can 
the pilot/aircraft in the 
that probability assigned to 
is that probability assigned 



19 



to no maneuver, and the assumed probabilitity distribution 
between these values is uniform with amplitude P (a) . 
Equation (2.12) is the correlation function which yields an 
acceleration time duration, R (a) , which is based on the 
magnitude of the acceleration |a|. H and t are simply 
correlation factors which determine how the R{a) varies over 
the range of possible acceleration amplitudes. From this 
model one can see how the duration of a high G maneuver for 
threat avoidance would be considerably less than for a low G 
clearing turn. 

C. GOBS MODEL IMPLEMENTATION 

The GDBS model was simulated on an IBM 370 in single 
precision Fortran. Figure 2.7 shows the basic flow diagram 
for the computer program, which implements this simulation 
model. The module labeled 'State Estimation Filter' 
represents those filter algorithms discussed in the next 
chapter. 
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Fig. 2.7. GDBS Model Flow DiagraM 
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III. AI RCRAF T STATE ESTIMATI ON 



A. BACKGROUND 

A great deal has been written on the theory and 
application of estimation filters. In particular# [2] 

provides a good overview of several such filters, including 
the alpha-beta and Kalman filters, and compares their 

relative performance not only in terms of accuracy and in 
response, but also in terms of computer implemenration costs 
in computation time and storage overhead. 

The general conclusion is that the Kalman filter 
oux-performs an alpha-beta filter of comparable order by 
about 2 to 1. However, the cost fcr such performance is 
increased computer computation time and memory, of the same 
relative magnitude. 

B. GENERAL DESCRIPTION OF THE ALPHA-BETA FILTER 

The basic theory of the alpha-beta filter is derived 
from minimizing the mean square error of the filtered 
states. A classic analysis of the alpha-beta filter is 

provided by [3]. The filter recursive equations are 

summarized below. 

X (k/k-1) =x (k-1/k-1) + T x(k-1/k-1) (3.1) 
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X (k/k) = X (kA-1) + a (z (k) - x(k/k-1) 



(3.2) 



x(k/k) = X (kA-1) + )3/T (z(k) - x(k/k-1)) (3.3) 

x(kA~1) is the predicted position, X (k/k) is the updated 
position, X (k/k) is the updated velocity, and z (k) is rhe 
noise contaminated measurement of position az the k-rh 
interval. T is the sample rate of the measurement process, 
a and ^ are usually fixed real constants. As pointed out in 
[41 these alpha-beta equations are analogous to a steady 
state Kalman filter. For typical parameter values the 
alpha-beta filter is simply low pass with a heavily damped 
time response. Thus the filter eliminates not only most 
high frequency measurement and process noise, but also most 
maneuver energy from the stats estimate. 

C. GENERAL DESCRIPTION OF THE KALMAN FILTER 

The Kalman filter generates a minimum variance estimate 
of the plant (aircraft) state vector when the measurement and 
plant process noise statistics are known and conform to the 
criteria shown below. 

E(V(k)7( j)^) = R (k) 6(k, j) (3.4) 
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E(A(W (k) W(j)^)A^) 



Q (k) 5(k, j) 



(3.5) 



E(V(k)R(j)^) = 0 



for all k, j 



(3.6) 



where 



5(k,j) 




k=j 



(3.7) 



A linear time- invari ant system is assumed, as in the 
discrete model representation shown in Figure 3.1. X(k) 
represents the (n x 1) state vector, Z (k) the (m x 1) output 
vector, <^ ( k) the state transition matrix, H (k) the (m x n) 
observation matrix, w ( k) state excitation or process noise, 
and V (k) the measurement noise. 

The Kalman filter recursion algorithm is summarized 
below. 

X(k+1) = 0(k)X(k) ♦ A(k)W(k) (3.8) 



Z (k) = H (k) X (k) + V(k) 



(3.9) 



X (k/k- 1) 



= 0(k,k-1)X(k-1/k-1) + A(k)U(k-l) (3. 1C) 
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I 



Dynamic Model 



I 



- 1 
1 
1 



Measurement Model 
v(k) 




I 

I 

I 

I 



Fig. 3.1. Dynamic System Hodel and Discrete Kalman Filter 



P(k/k-1) = <ji(k ,k-1 ) P (k-1/k- 1) 0(k,k- 1)^ + Q (k) (3.11) 

-L 

G(k) = P (k/k-1 ) H(k) [h (k) ? (k/k-1) H(k)^ + R (k)] (3.12) 
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^(k/k) = X(K/K-1)+G (k) jz (k) - H (k) X (k/k-lll (3.13) 

P(k/k) = (I - G(k) H(k)) P(k/k-1) (3.14) 

X(k/k-1) denotes the estimate of the state vector X(k) based 
on (k-1) measurements, Z(1),Z(2), Z(k-1). 

D. FILTER ORDER CONSIDERATIONS 

The filter order is chosen to match as closely as 
possible the expected plant dynamics of the system being 
modeled. A first order filter would be expected to estimate 
a constant velocity trajectory effectively and a second 
order filrer would accordingly observe a rrajectcry 
exhibiting constant acceleration. The order is used here in 
the mathematical sense and refers to rhe crder of the 
differential equation that defines the filter. 

Since the aircraft is known to be constrained to a 
constant velocity profile as it approaches the release 
point, it would seem reasonable to select a first order 
filter for modeling. The aircraft dynamics are anticipated 
to depart from the first order model enroure to the target 
thus creating transient errors which must be dealt with by 
filter adaptation. The alternative to this sxrategy is to 
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increase the order of the filter to observe these high 
energy maneuvers for state estimation. However, the 
settling time of a first order filter is generally less rhan 
that of a second order filter as discussed in[ 5 ] and 
graphically illustrated by the first and second order Kalman 
gain schedules shown in Figures 3.2 and 3.3 respectively. 




TIME (SEC) 



Fig. 3.2. First Order Kalman Gain Schedule 
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POSITION GRIN 



1.0 



0.8 



0.6 



0.4 



0.2 




TIME (SEC) 

Fig. 3.3. Second Order Kalman Filter Gain Schedule 



E. FILTE3 ADAPTATION 
1 . Backgr ound 

No matter what the order or the complexity of the 
filter type selected, it cannot be expected to fully model 
the aircraft dynamics and process noise covariance. The 
model is based on a linear time-invariant sys-^em and the 
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process noise is assumed to be stationary, white, and 
Gaussian. During portions of the flight, paricularly as the 
aircraft approaches the release point, the system dynamics 
are expected to approximate very closely the assumed model. 
However,, during other portions of the flight the aircraft 
dynamics are anticipated to depart significantly from the 
filter model. Certainly the pilot should not be constrained 
to behave in a manner consistent with the model, if the 

environment dictates otherwise. 

Since the pilot/aircraft dynamics are not fully 
modeled, the subopt imal filter that results might be 
expected to diverge, e.g. the error covariance generated by 
the filter and the actual error covariance become 

inconsistent. The desire is for the filter to transirion 
smcothly between accurate estimations, when the aircraft 
dynamics conform to those assumed for the model, and less 
accurate estimations, when the aircraft dynamics do not 
agree with the model. An adaptive filrer realizes this 
smooth zransition by adjusting filter parameters to vary the 
filter bandwidth to allow a more consistant match between 
the calculated and actual filter error covariances. 
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In the case of Kalman filter adaptation, the 
calculated error covariance becomes a function of the 
measured data indirectly by making the filter parameters 
dependent on the observed aircraft motion. The adaptive 
techniques for the alpha-beta filter are similar in concept. 
In either case the adaptive process is conceptually straight 
forward; first divergence is detected, then the filter 
parameters are modified. 

In the case of a ground directed bombing system of 
the type considered here, the aircraft’s behavior could 
depart from the filter model in a random fashion when the 
pilot maneuvers in response to a random event in the 
environment, or deterministically when he responds to target 
tearing inputs from the ground radar. These two situations 
may be treated separately or together for the purpose of 
filter adaptation. To treat them separately, as random and 
deterministic processes, requires knowledge of the 
pilot /aircraft response to target bearing inputs. In the 
case of the ground directed bombing system described in [ 1 ] 
this transfer function was known quite accurately since the 
input signals from the ground radar were directly coupled to 
the aircraft aerodynamic controls, with the uncertainties of 
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pilot response isolated from the contr.ol loop. With that 
information the deterministic forcing could simply be 
integrated into the filter model. Unfortunately such is not 
the case for this simulation. A unigue approach to this 
problem will be discussed later. The alternative approach 
is to consider both processes to be random and proceed from 
that assumption. 

2 . Innovations Statistics and Maneuver Detection 

As descibed in [5] the innovations or residual 
sequence of a filter can be observed in order to detect a 
bias that would indicate divergence of the stare estimate 
from the true state. This is given by 

l/(k/k-1) = Z(h) - H(k)X(k/k-1) (3.15) 

By substituting for Z ( k) from (3.9), the measurement model, 
we see that 

i;(k/k-1) = v(k) - H(k)€(k/k-1) (3,16) 

where 

€(k/k-1) = X(k/k-1) - X(k) (3.17) 

Taking expected values , we find that 

E (z/(k/k- 1) ) = 0 (3.18) 
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E(t/(kA-1) z/^(kA- 1) ) = 8(k) + H (k)P (k/k-1) H^(k) (3.19) 

# 

Thus by referring to the model statistics for R(k) and 
P(k/k-1), it becomes clear that when the system conforms to 
the system model, i.e. the filter is operating optimally, 
the innovation sequence should be zero-mean Gaussian with 
variance 

<7^ = + P(k/k-1) (3.20) 

^ One appraoch to adaptation considers the correlation 

of the innovation sequence, where the autocovariance 

i2^(i) = E(i/(k/k-1) t/(k-i/k-1-i)^) (3.21) 

should vanish for i^O. Based on these statistics, maneuver 

detection can be realized by observing the signs of the 
innovation sequence. The probability that a given sequence 
is either positive or negative is 

P( 0 > 1/ > 0 ) = .5^ (3.22) 

1-N 

Another approach utilizes (3.23) 

(k)| > c <T_ (k) (3.23) 

I •'n 

to declare a maneuver when exceeds a specified value, 

usually two or three standard deviations of C- . 
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Subsequent to maneuver detection, the filter 
parameters must be modified to correct filter divergence. 
Reference [51 summarizes numerous techniques, some being 
quite complex and computation intensive. The strategy 
chosen for this simulation was simply to reset the error 
covariance in response to a detected maneuver. In the 
situation of a ground directed bombing system, the resulting 
cost of a false detection becomes high only as the aircraft 
approaches the release point. This cost can be reduced by 
disabling filter adaption wirhin a specified time to go. 

Still another approach attempts to adapt the filter 
bandwidth by adjusting Q(k) in (3.11). This approach, 
investigated in [1] and [6] calculates Q (k) by 

Q(k) = a Del(k)Del(k) + b Del (k-1 ) Del (k- 1) (3.24) 

where a and b are determined by data analysis, and 

Del(k) = X(k/k) - '2(k/k-1) (3.25) 

ft variation of this technique that looks at only the change 
in the highest order state is investigated in the Kalman 
filter simulation. 

Most of the discussion thus far concerning filter 
adaptation has been directed toward Kalman filters. Most 
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approaches to adapting alpha-beta filter simply open the 
bandwith by switching to a different set of parameters when 
a maneuver has been detected. Reference [ 4 ] discusses an 
adaptation scheme that is more nearly optimal in the sense 
of covariance matching. However, for the sake of comparison 
the simple parameter switching technique is implemented in 
the alpha-beta filter simulation subroutine, since that is 
the approach used in the AN/TPB-1D. 

F, ESTIMATION OF PILOT RESPONSE TO TARGET BEARING INPUTS 

As discussed in the previous section, aircraft dynamics 
depart from the filter model randomly when the pilot 
responds to events in the environment and deterministically 
when he responds to target bearing inputs from the GDBS. If 
his response to these inputs were known with some degree of 
certainty, then deteministic forcing might be included in 
the filter model in a manner similar no that found in 
and [ 1 ], The importance of identifying parameters which 
define a system so that modern control strategies can be 
implemented is discussed in [7], In rhis case the 

paramerers would be those that describe the pilot/aircraft 
response to heading inputs. 
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By using an Autoregressive Moving Average (ARMA) 

representation for the pilot/aircraft system, and Kalman 
filtering to process the heading-in (actually heading error 
from the veiw point of the pilot), bankangle our data, the 
coefficients associated with the ARMA equation could be 
identified. From [7] we know that the pilot/aircraft system 

can be represented by the ARMA equation 

m n 

d(k) = E a . 6L(k-3) - Eb. d(k-3) (3.26) 

j=o J j=l ^ 

where the present bankangle output, 5 (k) , is a linear 
combination of pasr outputs, 5 (k-i) , and of past and 
present heading error inputs, d (k) . Estimating the 

coefficients of this ARMA equation can be formulated as an 
adaptive Kalman identifier, where the heading error and the 
bank angle are simple functions of rhe velocity and 
acceleration state estimates generated by the Kalman state 
estimation filter previously discussed. 

If the a. and b. coefficients of the ARMA equation are 
treated as states of the pilot/aircraft sysrem, then the 
state vector becomes 

— ^ (3.27) 
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He assume 



that these 



coefficients experience 



ra ndom 



perturbations so that 



a^ (k+1) = a^(k) + (k) 

bj(k+1) = bjCk) + w.(k) 



(3 



Equation (3.26) then becomes 



m 



d(k) = I^a. afk-j.) - I^b. d(k--j) + v (k) (3 



j=o ^ ^ 



j=l ^ 



where v^(k), Wj(k), v (k) are noise processes that have 
same statistics described for the Kalman filter earl: 
Combining (3.28) and (3.29) we have 



— — 






a (k+1) 




a (k) 


b (k+1) 




b (k) 


__ 




— . 



w(k) 



The measurment vector is defined, 

H(k) = ffl(k) 6i(k-1) . . . ^(k-m) - 

L f < « -I 

- d(k-l) ... - 5(k-n)J 

From [7] the solution is then formulated as 



I (k+1/k) 



= - S (k)H(k)"] 



I (k/k-1) 



b (k/k-1) 



b (k+1/k) 
where 

G (k) = P (k/k-1) H ^( k) Jh (k) P (k/k- 1) H^(k) + r] 



-1 



(3 



(3 



+ G(k) &(k) (3 



(3 



28) 

29) 

the 

er. 

30) 

31) 

32) 

33) 
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P (k/k) = P(k/k-1) - G(k) H (k) P (k/k-1) + Q 



(3 .34) 



Q = E 



Wi(k) 

w.(k) 



[ (k) (k) 



(3.35) 



R = E [v(k) v(k) 1 



(3.36) 



and 



r 



P (k/k-1) = E 





a(k)' 




a (k/k) 








a (k) 




a (k/k) 


iL 


b(k) 




6(k/k) 








b(k) 




S (k/k)J 



J 



(3.37) 



Initialization of the sta tes (coef ficients) and the error 
covariance would be similar zo that discussed in the next 



secrron. 



G. FILTER IMPLEMENTATIONS 

Three separate filter subroutines were developed to 
simulate the filtering of raw radar data generated by the 
ground radar of the bombing system previously described. 
All three filter configurations are oriented in nhe three 
dimensional Cartesian coordinate system described in Chapter 
2 since the aircraft dynamics are assumed to be more nearly 
linear and well behaved than in the polar coordinate system 
in which the measurements are generated. This disparity 
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between the measurement reference frame and the model 



dynamics reference frame results in a basic nonlinearity 
when transformations are required from one frame to the 
other. Probably a better coordinate frame for modeling 
aircraft motion would be one that translates with the 
aircraft and is oriented along the velocity vector. Such a 
coordinate system was found to be very awkward and difficult 
to implement, especially considering the problem of the 
transformation nonlinearity just mentioned. 

The first of these filters, designated ALPBTA, is a 
simple sixth order alpha-beta filter with the parameter 
switching adaptation tecnique described in the previous 
section. Adaption is initiated when a heading rate of 1 
degree per second is observed for period of 5 seconds or 
more. The second and third subrourines implement 

sixth (KALJ1N1) and n inth ( KALMN 2) order Kalman filrers 
respectively. Two separate adaptive techniques, which were 
described in the previous section, are included with each of 
these filters. The first of these adaptive algorithms, 
designated ADPTV1, adjusts the Q (k) matrix from changes 
computed in the highest order estimate. The second 
algorithm, designated ADPTV2, simply resets the covariance 
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of error matrix P (k) when a bias is detected in the 

innovations sequence for more than one second. As mentioned 
before, the difference between the sixth and ninth order 
filters is that the former do not estimate the acceleration 
states of the aircraft. The cost of this additional 

information provided by the ninth order filter is more 
computation time and computer memory. 

The aircraft model is formed by defining a 

three-dimensional Cartesian stare vector 



X3= [X y z]^ 



(3.38) 



where x, y, and z are each one-dimensional two element state 
vectors (position and velocity) for ALFBTA and KALflNI, and 
three element state vectors (position, velocity, and 
acceleration) for KALMN2. The state prediction equations 
are given by (3.1) for the alpha-beta filter and (3.10) for 
both Kalman filters. For the Kalman filter configuration 



0(k) 



0 0 0 

0 <t> 0 

0 0 <t> 



(3.39) 
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(k) 



(3.U0) 



4 0 0 

0 4 0 

0 0 4 



where (k) and 4 (Ic) are defined by (3.41) and (3.42) 'for 
KALMN1 and (3.43) and (3.44) for KALMN2. 



4>(k) = 



1 T 
0 1 



(3.41) 



4 (k) 



T/'2 

T 



(3 .42) 



«^(k) 



1 T T/^2 

0 1 T 

0 0 1 



I 



(3-. 43) 



4(k) 




(3.44) 



0 and A axe in general functions of k, however for 
this simulation they are not since a constant data rate is 
assumed and no extended predictions are required. The U (k) 
matrix would be utilized to include deterministic forcing in 
the model, if this information were available as in [ 1 ]. 
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However, since an adaptive Kalman identifier was not 
implemented to estimate the pilot/aircraft response, no 
attempt was made to include deterministic forcing in the 
model. 

1 . Kalman Filter Covariance of Measurement Noise. R 

Kalman filter theory assumes linear relationships 
among measurements and states as can be seen from (3.9). 
Since aircraft motion is modeled in a Cartesian reference 
frame and measurements are generated in a polar reference 
frame, the resulting relationships among the states and 
measured values are nonlinear, as can be seen from the 
transformation equations shown below. 

X = R cos(E) sin (A) (3.45) 

y = R cos(E) cos(A) (3.46) 

z = R sin (E) (3 .47) 

Using these polar/Car tesian transformations to 
nonlinearly combine the polar observations, 

three-dimensional Cartesian measurements are generated from 
(3.9) to form (3.48) below. 



4 1 



(3.48) 



2^ (k) 




R COS (E) sin (A) 




v^(k) 


Zy (k) 




R cos (E) cos (A) 


+ 


Vy (k) 


Zj (k) 




R sin(E) 




Vz(k) 



where the observation matrix for K&LMN1 is 



H (k) = 



1 0 0 0 0 0 
0 0 1 0 0 0 
0 0 0 0 1 0 



(3 .49) 



and for KALMN2 is 



H (k) = 



100000000 

000100000 

000000100 



(3.50) 



In order to compute the measurement error variance 
it is necessary to first linearize the measurement error. 
Differentiating equation (3.48) with respect to each of the 
measurement variables yields (3.51), where s and c represent 
sin and cosine respectively. 



J(x) = 



sAcE rcAcE 
cAcE -rsAcE 
sE 0 



-rsAs E 
-rcAs E 
rcE 



(3.51) 



Thus we find that the linearized Cartesian errors can be 
expressed as 



(3.52) 



V 






X 




R 




= J (X) 
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Therefore assuming no cross correlation of polar 
errors, the linearized Cartesian measurement error 
covariance matrix is 

R^(k) = J(x) Rp(k) JCx)”^ (3.53) 

The diagonal terms of R (k) are 

R(1,1) = r^ (a|sE^sA^ + a^cS^cA^) + a^cE^sA^ (3.54) 

R(2;2) =■ r^ (a|sS^cA^ + o|cE^sA^) + o^cE^cA^ (3.55). 

R(3,3) =. r^Og^ cE^ + a^sE^ (3-56) 

The off-diagonal elements'are 

R(1,2) = r^o^sE^sA^ ♦ ) cE^sAcA (3-57) 

R(1,3) = (o| - r^o| ) sEcEsA (3.58) 

R(2,3) + (a| - r^o| ) sEcEcA (3.59) 

and due to the symmetry of R (k) 

R (2, 1) = R (1 , 2 ) (3 .60) 

R (3, 1) = R (1 ,3) (3-61) 

R(3,2) = R(2,3) (3.62) 

It should, be noted than the R(k) marrix is nox consxant 
since ix depends on range, azimith, and elevaxion- 
2 . Filter Initialization 

All three filters were initialized with reasonable 
staxe values fcr position, velocity, and acceleration on the 
the firsx pass through the filter, since it is assumed xhat 
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the GDBS has maintained a good track for sometime prior to 
the final leg to the target. Consequently the covariance of 
error for the initial stats prediction vector is not set to 
an arbitrarily large number such as 10®, as is often done 
when there is little confidence in the initial state values. 
Instead 10^ is used since it is more consistent wi-h the 
covariance of error in good initial state values. This 
simulated pass from some other tracking filter to the filter 
of interest is realistic and reduces the settling time. 

Program constants and constant array calculations 
for the Kalman H, 4 , and Q matrices are set up on this 

first iteration. The process noise W is set at an 
arbitarily small number to ensure that the gain matrix will 
not converge to zero and accentuate divergence problems. 
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IV. PRESENTATION OF RESULTS 



A. gOALITATIVE OBSERVATIONS 

Seven different filter configurations were ultimately 
implemented and evaluated with the GDBS simulation. These 
configurations are discussed below and assigned 
alpha-numeric symbols for ease of reference. 

The sixth order alpha-beta filter, ALFBTA(AB) was 
implemented with the simple parameter switching technique 
outlined in Chapter 3. However, the heading rate maneuver 
detection process proved to be too insensitive to slow 
maneuvers and was therefore augmented with a arigger that 
reset filter parameters when the heading error exceeded 3 
degrees. The results of this change proved to be 
worthwhile, as will be shown later. 

The next three filters are variations of the sixth order 
KALMN1 filter. (K10) is KALMN1 without adaptation, (K11) is 
KALMN1 with the process noise adaptation scheme fcr modifing 
Q(k), and (K12) is KALNN1 adapted by resetting P(k), the 
error covariance matrix, as discussed in Chapter 3. Filters 
(K20) , (K21), and (K22) are variations of KALMN2 which 

correspond to the KALMN1 variants described above. 
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since the objective was to evaluate the accuracy and 
response of each filter in the final phase of bomb delivery, 
the simulation was initialized with the aircraft within 90 
seconds of the release point, at a speed of 480 knots, and 
on a heading within 10 degrees of the target bearing. For 
each run, after allowing 5 seconds for the filter to setrle, 
Tacan target bearing information was provided to the pilot 
controller. At 15 seconds elapsed time, filter adaptation 
was enabled and random maneuvers were begun at 20 seconds. 
Five separate runs were evaluated for each filter where the 
random maneuvers were ceased at 80, 60, 50, 40, and 30 

seconds prior to arrival at the release point. 

The following plots were generated for the case where 
maneuvers were stopped with a time-to-go of 50 seconds. 
Figures 4,1 through 4.21 show true and estimated (connected 
symbols) position, velocity, and acceleration trajectories 
as functions of time for all filter configurations. Note 
that the target position is designated by a circle on the 
horizontal trajectory plot. Also representative filter gain 
schedules are included to show the effects of the particular 
adaptation process being utilized. 
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Figures 4.1 and 4.2 indicate the magnitude of the 
maneuver encountered and the good state estimation qualifies 
of this simple sixth order alpha-beta filter. Notice there 
is no significant divergence of the estimated trajectory 
from the true trajectory throughout the run. Figure 4.3 
shows the step gain adaptation at the beginning of the run 
in response to the controlled turn to the target heading. 
This gain is then reduced after the turn is completed and 
before the first random maneuver begins, when the gain is 
again increased. 

The trajectory shown in Figure 4.4 contests sharply with 
that shown in 4.1, showing significant filter divergence for 
the nonadaptive sixth crder Kalman filter. The significant 
lag in velocity state estimation shown in Figure 4.5 results 
from the convergent gain properties characterized in 
Figure 4.6. 

The performance of K10 changes dramatically when it is 
made adaptive as shown in Figures 4.7, 4.9, and 4.9 for K11 
and Figures 4.10, 4.11, and 4.12 for K12. Figure 4.9 shows 
continuous gain adjustment in response to perceived changes 
in the process noise. Figure 4.12 shows the effect of 
resetting the covariance of error in response tc a maneuver. 
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Figures 4. 13, and 4.14 show the improvement in state 
estimation of the K20 nonadaptive Kalman filter when the 
order is increased over that of KIO. It is interesting to 
note that we see significant overshoot in the velocity 
estimate for the first time. Unlike the filters discussed 
thus far, K20 provides an acceleration estimate which can be 
seen in Figure 4.15 and accounts for the sensitivity of the 
velocity estimate. 

Adapting K20 through the noise process technique results 
in K21 which produces the position, velocity, and 
acceleration estimates shown in Figures 4.16, 4.17 and 4.18 
respectively. Figures 4.19, 4.20, and 4.21 provide the same 
information for K22, which represents rhe covariance of 
error adaptation variant of K20. These last two adaptive 
filters show little, if any, apparent improvement over the 
nonadaptive version. This observation is supported in the 
following section. 

B. QUANTITATIVE RESULTS 

A single run, for each filter configuration evaluated 
for each maneuver termination time, is not sufficient to 
properly determine filter performance over the range of 
possible maneuver trajectories and measurement noise 
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sequences. Therefore, 30 simulation trajectories per 
filter, per maneuver period, were conducted with different 
random maneuver and measurement noises sequences generated 
for each run. The bomb release signal to the pilot was 
assumed to occur at the closest point of approach (CPA) to 
the target release point. The average of the resulting 
CPA's for each 30 test runs are shown in Table I. CPA's 
greater than 250 feet are classified unsatisfactory and 
labled 'O' appropriately. 

At a glance it is apparent that the adaptive sixth order 
alpha-beta filter performs very well in such a dynamic 
environment, except when maneuvers are continued very close 
to the target. The nonadaptive sixth order Kalman filt==r is 
obviously unsuited by itself, but when made adaptive, 
preforms very well, particularly for the process noise 
adaptation technique when maneuvers are terminated late in 
the target run. The ninth order Kalman filter performance, 
both adaptive and nonadaptive, is comparable zc the 
alpha-beta and adaptive sixth order Kalman filters, but has 
problems in close due to ixs longer settling rime. Norice 
that the adaptive variants of the ninth order Kalman have 
little effect on that filter's performance, as we surmised 
in the last section. 
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TABLE I 



Release Point Error Table 
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Adp t V 




Adp tv 




Adp tv 


AB 


KIO 


Kll 


’ K12 


K20 


• K21 


K22 


80 


20 • 


46 • 


38 • 


39 ' 


30 • 


30 • 


30 • 


60 


43 • 


u 


42 * 


37 ’ 


43 • 


39 • 


35 • 


50 


41 • 


u 


75 • 


53 • 


50 ’ 


65 • 


52 ' 


40 


55 ' 


u 


54 ' 


82 • 


119' 


98 ' 
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30 
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109 • 


a 
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u 
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Table II shows the relative cost, in computation time 
and memory, to implement KALMNl and KALMN2 in relation to 
ALFBTA. 
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TABLE II 



Relative Computation and Memory Costs 



Alpha-Beta (sixth ord) 


1 


Kalman (sixth ord) 


2 . 9 


Kalman (ninth ord) 


3 • 4 



The only advantage to implementing the most responsive 
variant of KALHN1 would be to reduce the probability of a 
GDBS generated abort, due to large predicted bomb impact 
errors, when maneuvers are carried very close to the release 
point. Lastly, if the adaptive Kalman identifier proved to 
be useful in providing deterministic forcing for the Kalman 
filter model and resulted in improved accuracy and response 
over the alternatives presented, the cost in computation and 
memory resources would be even greater than we have seen 
here. 
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Fig. U.1, ALFBTA(AB) Horizontal Position Trajectory 
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Fig. 4.2. ALFBTA(AB) True and Estimated Y-7elocity 
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Fig. 4,3, ALFBTA(AB) T-Gain Schedule 
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Fig. 4.4. KALHNI(KIO) Horizontal Position Trajectory 
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Fig« 4,5. K&L21N1 (K10) True and Ss'timatsd Y^Telocity 
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Fig. 4.6. KALMNI(KIO) Y-Gain Schedule 
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F-g. 4.7. KALMNI(KII) Horizontal Position Trajectory 
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Pig- 4.8. KALMN1 (K11) True and Estimated T-7elocrity 
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Fig. 4.9. KALHNI(KII) T-Gain Schedule 
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Fig. 4,10. KALMN1(K12) Horizontal Position Trajectory 
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Fig. 4.11. KALMN1(K12) True and Estimated Y-Velocity 
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Fig. 4.12. KALHH1(K12) Y-Gain Schedule 
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Fig. 4.13. KALHN2(K20) Horizontal Position Trajectory 
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Fig. 4.14. KaLMN2(K20) True and Estimated T-7elocity 
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4.15. KALMN2(K20) True and Estimated Y-Accelerati on 
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Fig. 4-16, KALMN2(21) Horizontal Position Trajectory 
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Fig. 4.17. KALMN2(K21) True and Estimated Y-Velocity 
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Fig. 4.18. K&LMN2 (K2 1) True and Estiaated Y-4ccelerati on 
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Fig. 4.19. KALMN2{K22) Horizontal Position Trajectory 
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4.20. KALMN2(K22) True and Estimated Y-*7elocity 
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Fig. 4.21 



KALHN(K22) True and Estimated Y-Accelerati on 
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C1c******#gR0UND directed bombing system simulation******^*^*** 



IMPORTANT variables DEFINED: 



TACRRXI STATE, TIME ) 
TACRRY(STATE,TIMEJ 
TACRPZ(STATE,TIMEJ 
TACACXI '• , " } 

TACACYI •• , " ) 

TACACZi " , " i 

TACREXI ” , '• ) 

TACREYI «• , ” ) 

•I , !• J 



TRUE A/C STATE<1=P0SIT,2=VEL, 
3=ACCEU <3 TIMSINJ IN RADAR 
CARTESIAN COGRD REF FRAME. 

TRUE A/C STATE! AT 

TIME(N) IN A/C CARTESIAN COORD 
REFERENCE FRAME. 

EST. AIRCRAFT STATE ) AT 

TIME(N) IN A/C CARTESIAN COORD 
REFERENCE FRAME. 

POSITION(XJ IN ROR. 



■ACREZI 

TACRMX - MEASURED A/C POSITION(XJ IN ROR. FRAME 

T4CRMY - MEASURED A/C POSITIONIYJ IN RDR. FRAME 

TACRMZ - MEASURED A/C POSITICN(Z) IN ROR. FRAME 

ROSFQ - RADAR DATA SAMPLE FREQUENCY. 

OT - RADAR SAMPLE INTERVAL TIME. 

N - N-TH ITERATION AT SAMPLE RATE OT. 

TACHDG - TRUE AIRCRAFTIACi HEADING IN RADAR FRAME. 
TACVEL - TRUE AIRCRAFT VELOCITY IN RADAR FRAME. 

TTG - TIME TO GO UNTIL MOT TGT. 

HDGERP - DIFFERENCE BETWEEN OSRDGT AND TACHDG. 
ELPSTM - ELAPSE TIME IN SECONDS SINCE BEGINNING OF 
SIMULATION. 

SRTMVP - START MANEUVERS ; A/C MANEUVERING CAN BEGIN. 
STPMVP - STOP MANEUVERS; A/C MANEUVERING MUST STOP. 
MNVRPT - MANEUVERING PILOT. 

CNTLPT - CCNTROLED PILOT. 

MODEL - FREE INERTIA MODEL OF A/C WITHOUT WIND 
EFFECTS. 

BNKANG - AIRCRAFT BANKANGLE. 

NTIMES - NUMBER OF TIMES THRU SIMULATICM LOOP. 

GTRNG - RANGE(RNG) FROM A/C TO TGT ALONG DESIRED 
GRCUNO TRACK. 



- TARGET COCRO. IN RDR FRAME 



TRUE 

TRUE 

EST. 

EST. 



WI ND 
WIND 
WIND 
WIND 



DIRECTION IN RADAR FRAME 
VELOCITY IN RADAR FRAME 
DIRECTION IN RADAR FRAME 
VELOCITY IN RADAR FRAME 



TGTRRXl 
TGTRRY 
TGTRRZ 
TWDRR - 
TWVRR - 
TWDRP - 
TWVRR - 
TWVRRX 
TWVRRY 
EWVRPX 
EWVRRY 
X2SIGM 
Y2SIGM 
Z2SIGM; 

PINITL - ERROR COVARIANCE DIAGONAL INITIAL VALUE 
GX 

- FILTER 



- TRUE WIND X-Y VELOCITY COMPONENTS 

- EST. WIND X-Y VELOCITY COMPONENTS 



- PROCESS NOISE VARIANCE 



GY 
GZ 

XPES 
YRES 
ZPES^ 
RAORES - 



X,Y,Z GAIN VALUES AT TIME N 



- FILTER X,Y,Z RESIDUE VALUES AT TIME N 



RADIAL RESIOU AT TIME N 



IMPLICIT REAL! A-H,C-Z) , INTEGER! I-N) 
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DOUBLE PPECISICN CSEED 

COMMON/ DCUBLE/DSEED 

COMMON/TMRPST/N,TACRRX ( StlOOOJ t TACRRYO , 1000 ) , TACRRZ ( 
+3» 1000) 

C0MM0N/TMACST/NT,TACACX(3» 1000 i t TACACYt 3, 1000 J tlACACZX 
+3, 1000) 

CQMM0N/TGTWN0/TGTRRX,TGTRRY,TGTRRZtTW0RRf TWVRRfEWORR t 
+EWVRR 

COMMON/ PI LOT/ TB,SRTMVR,STPMVR»0T 

C0MM0N/N0ISE/TACRMX( 1000) fTACRMY( 1000), TACRMZ( lOCO) 

COMMON/ FILTSR/TACREX(3 , 1000 ) , TACREY ( 3,1000) , TACREZ ( 3 , 

+ 1000 ) 

COMMON/ PR0CES/X2SIGM.Y2SIGM,Z2SJGM,EWVRRX,£WVRRY,GX( 
+1000) ,GY( 1000) ,GZ( 1000) ,RADRES< 1000) ,XRES<1000) 

+, YRES ( 1 000 ),ZRES( 1000) ,W11( 1000) ,W22 <1000), W33( 1000), 
+P( 1000,9) tPINiTL 

COMMON/ PARAMT/G,G2,GK1,GK2 

DIMENSION X(1000),Y( 1000 ) , TGTXd ) , TGTY< 1 ) , TRUACC( 1000) 
+, TIME (1000) ,X1(1000) ,Y1 ( 1000) ,ESTACC (1000) 



INITIALIZE PARAMETERS; 

3NKANG=0,0 

0EA0ZN=0.0 

OLDHDE=0.0 

K = 0 

K1=0 

K2=l 

T8=2.C 

PINITL=1000. 

SRTMVR=20.C 
STPMVR=50. 0 
IR0SFQ=8 
TTG=999. 

NT!MES=800 

SECON0=0.0 

HOG£RP=0.0 

G=32. 174049 

G2=16.0870C7 

GK1=3.5 

GK2=3.5 

PI =3.141592654 
OLDHDE=0.0 
0SEED= 1.0 CO 
RSIGMA=100. 

ASIGMA=.001 

£SIGMA=.001 

kfiltr=o 

KA0APT=0 

X2SIGM=1. 

Y2SIGM=1. 

Z2SIGM=1. 

READ INITIAL AIRCRAFT STATES IN AC REF. FRAME. 
WRITE(6,6) 

READ (5, 1) TACACX( 1,1) ,TACACY( 1,1 ) , TACACZ ( 1 , 1 ) 
REA0(5, 1) TACACX(2, 1 ),TACACY(2, 1),TACACZ( 2, 1) 
READ (5, 1) TACACX(3,1 ) ,TACACY(3,i ) ,TACACZ(3,1 ) 



OBTAIN X C Y WIND COMPONENTS IN RADAR FRAME £1 ADD TO 
CORRESPONDING A/C INITIAL VELOCITY STATES TO OBTAIN 
INITIAL AIRCRAFT STATES IN RADAR REFERENCE FRAME. 
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3 

41 

C 



c 

c 

c 

c 

c 



c 

c 

c 

c 

c 



c 

c 

c 



45 



46 

59 



REA0(5,2 ) TGTPRX»TGTRRYtTGTRRZ,TWDPR»TWVRR,EWORR,eWVRi^ 
TWVRRX=TWVRR* SI N(TWORR*P 1/180. 0**1.687805556 
TWVRRY=TWVRR*C0S(TWDRR*PI/1 80. 0**1.687805556 
EWVRRX=EWVRR*SIN( EW0RR*P 1/180. 0**1. 687805556 
EWVRRY=EWVRR*C0S(EHDRR*PI/180.0**1.687805556 
00 41 1=1,3 

‘ ' (I .NE. 2) GC TO 3 



IF 



TACRRXI 2,1)=TACACX(2,1* 
TACRRY< 2,l)=TACACY<2,i) 
TACRR2< 2,1)=TACACZ<2,1 J 



TWVRRX 

TWVRRY 



GO TO 41 
CONTINUE 
TACRRXd ,1 
TACRRY( I ,1 
TACRRZd ,1 
CONTINUE 



)=TACACX( 1,1* 
)=TACACY( I ,1) 
*=TACACZ( 1,1* 



WRITE 

WRITE 

WRITE 

WRITE 

WRITE 

writf 

WRITE 
WR ITE 
WRITE 
WRITE 
WRITE 

WRITS 

WRITE 

WRITE 

WRITE 

WRITE 

WRITE 

WRITE 



( 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 

< 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 

( 6 , 



4) 

6 ) 

8 ) 

10) 

12 ) 

14) 

16) 

18) 

20 ) 

21 ) 

22 * 

24) 

26) 

50) 

53) 

60) 

63) 

65) 



TACRRXd, 1) ,TACRRY(1, 1 * , TACRR Z d , 1 ) 
TACRRXI 2,1) ,TACRRY(2,1* ,TACRRZ(2,1) 
TWDRR,TWVRR 
EWCRR,£WVRR 
R S IGMA,A SI GMA,E SIGMA 
X2SIGM,Y2SI6M,Z2SIGM 
PINITL 
KFILTR 
KAOAPT 
SRTMVR 
STPMVR 



DETERMINE RADAR SAMPLE I NTERVAL C CT ) . 

DT=1.0/FLQAT( IRDSFQ) 

PERFORM SIMULATION LOOP N-TIMES. 

DO 900 N = 1, NTIMES 
NT=n 

ELPSTM = (N-1) * DT 
TIME(N)=ELPSTM 

NOW CALL PLRNGI TO TRANSFORM RADAR CARTESIAN COORDS 
IN POLAR, ADO WHITE NOISE, AND RETRANSFORM INTO RDR 
CARTESIAN COORDS IN PREPARATION FOR FILTERING. 



CALL PLRNO HR SIGMA, AS IGMA,ESIGMA, RANGE, AZMI TH, 
+ELEVTN) 



I«= ( KFILTR .NE. 0) GO TO 45 
CALL ALFBTA(DT,HOG£RR,TTG* 

GO TO 59 
CONTINUE 

IF (KFILTR .NE. 1) GO TO 46 

CALL KALMNKKADAPT, DT,RSIGMA,ASIGMA,ES IGMA, RANGE, 
+AZMITH, ELEVTN,TTG) 

GO TO 59 
CONTINUE 

CALL KALMN2I KAOAPT, OT ,RS I GMA , ASI GMA, ES IGMA , RANGE , 
+AZMITh,ELEVTN,TTG) 

CONTINUE 
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550 

580 



NOW CALL TACAN SUBROUTINE TO OBTAIN HEADING TO FLY 
TO TGT. 

CALL TACAN (TACHDG,DSROGT,TACVEL,TTG»GTRNGtHOGERRJ 

IF (N .GT. 800i GO TO 580 

IF (MODIN-1,8) .N£. OJ GO TO 550 

WRIT E (6 t 75) ELPSTMtTACRRXI 1»N) ,TACRRY(1,N), 
+TACRRZ(1 »N) »TACRRX(2»N) tTACRRY(2 ,N) ♦TACRRZ(2,N) , 
+TACRRX(3,N) ,TACRRY(3»N) ,TACRRZ( 3tN) 

WRITE (6 t 78) TACREXIitN) »TACREY ( 1 ,N ) , TACRE2 ( 
♦1,N),TACREX(2 tN) tTACREYIZfNJ ,TACREZI 2fN) , 

+TACREX(3,N) ,T ACREY 1 3 ,N J , TACREZ( 3 tN) 

WRITE (6, 78) TTG,TACRMX( N) ,TACRMY(N) ,TACRMZ(N) 
♦,HDGERRtCSRDGT,TACHDGiTACVEL,GTRNG 

WRITE (6, 78) GX(N),GY(N),GZ(N),XRES(N) ,YRES(N) 
♦•,ZRES{N)»W11(N),W22(N) ,W33(N) 

WRITE (6, 78) P(N,1) ,P(N» 2) »P(N,3) ,P(N,4) , 
+P(N,5) ,P(Nt6) ,P(N»7) ,P(N,8) tP(N,9) 

CONTINUE 

CONTINUE 



NO CONTROL 
IF (ELPSTM 



INPUT UNTIL FILTER SETTLES. 
.LT. 5.) GO TO 800 



DETERMINE CONTROL INPUT TO AC. 

NO CONTROL INPUT IF TTG IS LESS THAN 3 SECOND OR IF 
CLOSEST POINT OF APPROACH! CPA ) HAS BEEN REACHED. 



CALL TTGCPA(N,K,TTGtGTRNGtIFLAG) 



IP (((IFLAG) .EQ. 1) .AND. (ABS(HCGSRR) .GT. DEADZN 
+)) GO TO 600 

TACACX(3,N)=0.0 
TACACY( 3,N)=C.O 
TACACZ<3,N)=0.0 
GO TO 800 

CHOOSE MNVRPT OR CNTLPT RESPONSE BASED ON 
MAGNITUDE CF HDGERR,ANO SRTMVR ANC STPMVR CRITERIA. 

600 CONTINUE 

IF ( (ABS(HOGERR) ) .GT. .525598776) GO TO 700 

IF ((ELPSTM .LT. SRTMVR). OR. (TTG .LT. STPMVR)) 
+G0 TO 700 

CALL MNVRPT(Kl) 

GO TO 8C0 

CALL CNTLPT SUBROUTINE IF MANEUVER NOT PERMITTED. 



700 



800 



C 



CONTINUE 

CALL CNTLPT(K2rHDGERR) 

PROVIDE MODEL SUBROUTINE AC STATES AT TIME N. 
CALL XMODEL 

MODEL RETURNS STATES IN AC FRAME FOR TIME N+1. 

ADD WIND EFFECTS OVER DT TO OBTAIN AC STATES IN 
RADAR FRAME. 

CALL WNDEFF(TWVRRX»TWVRRYtOT) 
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1 

2 

4 

6 

8 

10 

12 

14 

16 

13 

20 

21 

22 

24 

26 

50 



53 

60 

63 

65 



75 

78 



CONTINUE 
STOP 

FORMATOFIO.O) 

FORMAT(7F10.0) 

FORMAT(58X, «GCBS SIMULATION*) 

FORMAT (/////2X,» INITIAL CONDITIONS:* ) 
F0RMAT(//5X, • A/C INITIAL POSITION IN 
+ SYST5M(FT) = * ,3( lOX, FI 0 .3J / ) 

*A/C INITIAL VELOCITY IN RADAR 
= * »3(10X,F10.3)//) 

’TRUE WIND =*tl0X»F10.3 



RADAR REFERENCE 



REFERENCE 



F9.3/) 

*EST. WIND =*,10X,F10.3,*/*,1X,F9.3//} 



»*/*tiX, 



FO PMAT(5X, 

+SYSTEM(FT) 

F0RMAT(5X, 

F0RMAT{5X, , . _ . , 

FGRMAT( 5X» ‘MEASUREMENT NOISE VARI ANCESCR ( FT) » A< RAD) 
♦E(RAO)) =* 

F0RMAT(5X, 

+3( 10X,F1C.3)/) 

F0R.MAT(5X, *ERPCR COVARIANCE DIAGONAL =* 1 10X» F10.3// ) 
F0RMAT(5X, *FILTER DESIGNATION =',4X,I1/) 

F0RMAT(5X, ‘FILTER ADAPTATION =*,5X,I1//) 

F0RMAT(5Xt ‘START MANEUVER = * , lOX , FIO .3/ ) 

F0RMAT(5X, * STOP MANEUVER ' '' 



,3( 10XtF10,3)/) 

‘PROCESS NOISE VAR lANC ES ( X , Y, Z ) =* 



16X, 



FORMAT! *0* ,6X ,* SECOND* 
+*TACRRZ1* ,6X, ‘TACRRX2* 
+1TACRRX3* ,6X, ‘TACRRY3* 



=* ,10X,rl0,3//) 



,6X, ‘TACRRXl* 
,6 Xt ‘IACRRY2* 
6X 



6Xf*TACRRYl* 
‘TACRRZ2* 



» 6 X» 

) 

* ,6X, *TACREZ1‘ 
t6X, 



,6X, 

»6X, 



tOA» • IMUrVATJ* » O A » * T ACRR Z3 * 

FORMAT! *0S 18X,*TACREX1* ,6X, ‘TACREYl 
+ ,*TACREX2* ,6X,*TACREY2* ,6X,*TACREZ2* 

♦ ‘TACREX3* t6Xt ‘TACREY3* f 6X» * TACREZ3* ) 

FORMAT! *0* t22X, *TTG* ,7X,*TACRMX* »7X, ‘TACRMY* ,7X, 

+ *T ACRMZ* ,7X,* FOGERR* r7X, ‘DSROGT* »7X, 

+ *TACHDG* t7Xt*TACVEL* »8X,*GTRNG* ) 

FORMAT! * 0* , 19 X , • X-GA IN * » 7X , * Y-GA IN * ♦ 7X, * Z-GAIN* ,5X, 
+ *X-RESIDU* ,5Xt*Y-RESI0U* ,5X ♦ ' Z-RESI DU * 1 7 X, * W ! 1 , 1 ) * 



6X 



+,7X,‘W!2»2)*,7X,*W!3»3)*) 

FORMAT! *0* t20X,* PKKll* , 8X» * PKK2 2 * , 8X , *OKK33* 
+‘PKK44* ,3Xt*PKK55* ,8X»*PKK66* ,8X,*PKK77* ,8X, 
+ »3X, * PKK99*///) 



3X, 

PKK88* 



FORMAT! ///10F13. 4) 

FORMAT! 13X,9F13. 4) 

END 

SUBROUTINE TACAN! TACHDGt DSROGT , T ACVEL »TTG, GTRNG, 
+HOGERR) 

IMPLICIT REAL (A-H,0-Z), INTEGER! I-N) 

nnUBLE PRECISION DSEED 
COMMON/ DOUeLE/CSEED 

COMMCN/TMRRST/N,TACRRX!3tlOOO) fTACRRY! 3»1000) » 
+TACRRZ(3»1000) 

COMMCN/TGTWND/TGTRRX,TGTRRY,TGTRRZfTWORR,TWVRR, 

+ EWDRR tEWVRR 

COMMON/ PIL0T/T3,SRTMVR,ST?MVR,DT 
C0MMCM/FlLTER/TACR£X!3rl000) » TACREY ! 3 , 1000 ) t 
+TACREZ! 3, 1000 ) 

C0MMCN/PARAMT/G»G2. GK1tGK2 

01=3.141592654 
EWO=EWDRR*PI/ 180.0 
=WV=EWVRR’^ 1.68780556 

TACVEL=SQRT!TACREX! 2,N)*TACREX!2tN)+TACREY! 2tN)* 
+TACREY!2tN) ) 

TACH0G=TRUhDG! TACREX!2f N) »TACREY! 2,N) ) 
TGTDIR=TRUFCG! !TGTRRX-TACREX! 1,N) ) , !TGTRRY- 
+TACREY! l.N) ) ) 

0SRDGT=TGTCIP 

HOGERR=CSRDGT-TAChDG 

GTRNG=SQRT! !TACREX! 1 ,N)-TGTRRX )**2+! TACREY ! l,N )- 
+TGTRRY)=f*2) 

TTG=GTRNG/TACVEL 
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600 

700 

800 

900 

90 

100 



200 



RETURN 

END 



SUBROUTINE TTGCPA ( N , K , TTG ,RNGt I F LAG) 

IF ( (TTG .GT« 3«0) .OR. (K .EQ. 0)) GO TO 600 
GO TO 700 
CONTINUE 

IF (N .LT. 1001 0L0RNG=RNG 
DIFFNC=0LDRNG-RNG 
IF (TTG .GT. 10.) GO TO 800 
IF (DIFFNC) 700,800t800 
IFLAG=-1 
K=1 

GO TO 9C0 
IFLAG=1 
CONTINUE 
OLDBNG=RNG 
RETURN 

END 



SUBROUTINE MNVRPT(Kl) 

DOUBLE PRECISION DSEEC 
COMMON/OOUBLE/DSEED 

C0NM0N/TMACST/N,TACACX(3, 1000 ),TACACY<3,1000) , 
+TACACZ( 3,10C0) 

CnMMCN/PILCT/TBtSRTMVR,STPMVR,OT 
C0MM0N/PARAMT/G,G2 tGK 1,GK2 
IF ( (N-Kl) .EQ. 1) GO TO 100 
CMDACC=C.0 
N1=0 

PA=GGUBFS( OSEED) 

AMXACC=14.0*(PA-.5) 

ACC0UR=20.0*EXP( .25* ABS ( AMXACC I ) 

CONTINUE 

ACCOUR=ACCCUR-CT 

IF (ACCDUR .LE. 0.0) GO TO 90 

CMCACC=AMXACC*( l.-EXP(-0T/TB) )+CMOACC*EXP ( -OT/TB ) 
AACK0G=TRUFDG(TACACX(2»N) ,TACACY( 2fN) ) 

TACACXI 3»N)=CM0ACC*CCS(AACH0G)*G 

TACACY(3,N)=-CM0ACC*SIN( AACH0G)*G 

TACACZC 3t10CO)=C.O 

K1=N 

RETURN 

END 



SUBROUTINE CNTLPTI K2 ,H0GERR ) 

GENERATE CGNTROLEO PILOT BANKANGLE. 

PHOl IS COMPONENT BASED ON ANGLE ERROR HDE. 

PHD2 IS COMPONENT BASED ON ANGLE ERROR RATS hDEDOT. 
3H0 IS OeSlPED PILOT GENERATED BANK ANGLE. 

DOUBLE PRECISION OSEED 
CO MMON/OCU 3 LE/CSE ED 

COMMON/TMACST/N,TACACX(3»1000)»TACACY(3,1000) , 
+TACACZ(3,1000) 

C0MMCN/PARAMT/G»G2.GK1,GK2 
COMMON/ PILCT/TB,SRTMVR,STPMVR»DT 
IF ((N-K2) .EQ. 1) GO TO 200 
OLOHDS=0.0 
8NKANG=C .0 
N2=0 

CONTINUE 
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N2=N2+1 

HO£OCT=(HDGERR-OLOHCE)/DT 

PH01=GK1*HDGERR 

PH02=GK2*HOEDOT 

CMDANG=PHD1+PH02 

IF <ABS(HOGERR) .GT. .1745) 



GC TO 202 
CM0ANG=.5236 
CM0ANG=-,5236 



CMDANG=1.047 
CM0ANG=- 1.047 



IF (CMDANG .GE. .5236) 

IF ICMDANG .L£. -.5236) 

GO TO 204 
202 CONTINUE 

IF (CMDANG .GE. 1.047) 

IF (CMDANG .LE. -1.047) 

204 CONTINUE 

BNKANG=CMDANG*( l.-EXP(-0T/TB) )+BNKANG*EXP( -DT/TB ) 
AACVEL=SQRT(TACACX( 2,N)*TACACX(2tNi+TACACY(2»N)* 
+ TACACYC 2tNI ) 

AACHCG=TRUHDG(TACACX(2tN) ,TACACY(2»N) ) 

H0GRAT=TAN( BNKANG)*G/AACVEL 

TACACX(3tN)=AACVEL*HDGRAT*C0S(AACHCG) 

TACACY(3,N)=-AACVEL*H0GRAT*SIN(AACFDG) 

TACACZ(3,N)=0.0 

0L0H0E=HDGERR 

K2=N 

RETURN 

END 



SUBROUTINE XMODEL 

DOUBLE PRECISION DSEEO 
COMMCN/OOU6LE/CSEED 

C0MMCN/TMACST/N,TACACX(3 flOOO ),TACACY(3»1000) » 
+TACACZ(3,1000 ) 

COMMON/PILCT/TB,SRTMVR,STPMVR ,0T 
C0MM0N/PR0CES/X2SIGM,Y2SIGM,Z2SIGM 
COMMON/ PARAMT/G,G2tGKl,GK2 

DIMENSION PHI(6»6)»D£L(6,3) »X(6»l),U(3a) tF(6,l) 
+XNU6, 1) tXl(6, 1) 

T2=( CT*0T) /2 

IF (N .NE. 1) GO TO 160 

COMPUTE STATE TRANSITION MATRIX(PHI). 
INITIALIZE MATRIX 3 0.0 , 1.0 ON DIAGONAL. 

00 130 I=lt6 
00 120 J=l»6 
PHK I,J)=0.0 

IF ( I .EQ. J) PHI tl ,J) = 1.0 
120 CONTINUE 

130 CONTINUE 

PHI(1,2)=0T 
PHI(3,4)=0T 
PHK 5»6) = 0T 

COMPUTE DEL MATRIX(DEL) 

INITIALIZE a 0.0. 

DO 150 1=1,6 
00 140 J=l,3 
DEL(I,J)=0.0 
140 CONTINUE 

150 CONTINUE 

0EL( l,l)=T2 
0EL(3,2)=T2 
0EL(5,3)=T2 
0EL(2,1)=DT 
0EL(4,2 )=DT 
0EL(6,3)=DT 



SET UP 'X* MATRIX. 
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160 



C 

c 



X{1»1)=TACACX(1,N) 

X(2,1J=TACACX(2,NJ 
X(3» 1)=TACACY( 1»N) 

X(A,1)=TACACY(2,N) 

X(5,1)=TACACZ( 1,N) 

X(6, l)=TACACZ(2,N) 

CALL PRODUCT SUBROUTINE TO MULTIPLY PHI & X 
CALL PRCCCT(PHI,X,6,1,6, XU 

SET UP 'U* MATRIX. 

U(i»l) = TACACXO,N) 

U(2» 1)=TACACY( 3,Ni 
U(3,1)=TACACZ( 3,N) 



CALL PRODUCT SUBROUTINE TO 
CALL PRCDCT(0EL,U,3,1,6,F J 

CALL ADD SUBROUTINE TO AOO 
STATES AT TIME (N+1). 

CALL A00(X1,F,1,6,XN1) 



MULTIPLY DEL S U 



X 6 F TO OBTAIN 



END 



IP=N+1 

TACACX( 1,IP) 
TACACX( 2,IP) 
TACACX(3,IPJ 
TACACYI 1,IP) 
TACACY( 2tIPl 
TACACY(3,IP) 
TACACZ( 1,IP) 
TACACZ( 2,IP1 
TACACZ(3 ,IP) 

RETURN 



= XNK1,1 ) 

=XN1(2,1) 

=TACACX(3,N) 

=XN1(3,U 

=XNU 

=TACACY(3,N) 

=XN1(5,1) 

= XNU6,U 
=TACACZ(3tN) 



THIS SUBROUTINE COMPUTES THE MATRIX PRODUCT A*6 AND 
STORES RESULT IN C. 

SUBROUTINE PRCCCT { A, B, N t M, L ,C ) 

DIMENSION A(L ,N) ,6(N,M) ,C( LfM) 



170 

180 



INITIALIZE *C* 

00 180 1=1, L 
DC 170 J=1,M 
C( I, J>=0.0 
CONTINUE 
CONTINUE 

MULTIPLY. 



MATRIXa 0.0, 



190 

200 

210 



END 



DO 210 1=1, L 
00 200 J=1,N 
DC 190 K=1,M 

C( I,KJ=C(I,K)+A(I,J)*8< J,K) 
CONTINUE 
CONTINUE 
CONTINUE 
RETURN 
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213 

215 



220 

230 



SUBROUTINE ADO ( A » B tL tM , C ) 



8<M,LJ ,C(M,L) 

0 . 0 . 



END 



DIMENSION A(MtU, 
INITIALIZE *C* a 
00 215 I=ltM 
DO 213 J=1,L 
C<I, J)=0.0 
CONTINUE 
CONTINUE 

NOW ADD *A* S *B' 



00 230 1=1, M 
DO 220 J=1,L 

C(I, J)=A(I,J)+B(I,J) 
CONTINUE 
CONTINUE 
RETURN 



SUBROUTINE WNCEFF ( TWVRRX ,TWVRR Y , DT ) 

C0MMCN/TMACST/NT,TACACX(3, 1000i,TACACY(3, 1000) 
+TACACZ(3,1000) 

COMMON/TMRRST/N,TACRRX(3,100CJ,TACRRY(3,10C0) , 
+ TACRRZO ,1000) 

IP=N+1 

TACRRX(1,IP)=TACACX(1,IP)+0T*TWVRRX*N 
TACRRX( 2,IP)=TACACX(2,IP)+TWVRRX 
TACRRX(3,IP)=TACACX(3,IP) 

TACRRY( 1 ,1 P ) = TACACY( 1, IP )+DT^TWVRRY*N 
TACRRY(2,I P)=TACACY(2,IP)+TWVRRY 
TACRRY(3,IP ) = TACACY( 3,IP ) 

TACRRZ( 1 ,I P)=TACACZ( 1 ,IP) 
TACRRZ(2,IP)=TACACZ(2,IP) 

TACRRZ(3,IP ) = TACACZ(3,IP ) 

RETURN 

END 



31 

41 

51 

61 

71 

91 

101 

111 

121 

131 



FUNCTION TRUHCG(XVEL,YVEL) 

01 =3,141592654 
IF (XVEL) 31,41,51 
IF (YVEL) 71,111,71 
I<= (YVEL) 1C1,91,121 
IF (YVEL) 61,91,61 

TRUHDG=P 1/2.0 - ATAN ( YVEL/XVEL ) 

GO TO 131 

TRUHDG=3.0*PI/2.C - AT AN ( YVEL/ XVEL ) 
GO TO 131 
TRUHOG=PI/2.0 
GO TO 131 
TRUH0G=PI 
GO TO 131 
TRUH0G=3.0*PI/2.0 
GO TO 131 
TRUHDG=0.0 
RETURN 

END 



THIS SUBROUTINE TRANSFORMS CARTESIAN COORDS INTO 
POLAR IN THE RCR REF. FRAME THEN ADOS WHITE NOIS 
0 MEAN AMD VARIANCE, RS IGMA, AS I GMA, AND ESIGMA T 
RANGE, AZIMITH, AND ELEVATION RESPECTIVELY. THES 
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CONSTITUTE THE MEASURED POSIT OF THE A/C, THESE NOISY 
POLAR MEASUREMENTS ARE THEN TRANSFORMED BACK INTO THE 
RADAR CARTESIAN SYSTEM IN PREPARATION FOR FILTERING. 



SUBROUTINE PLRNO U RS IGMA,AS IGMA , ES IGMA» RNGPNS t AZI PNS , 
+ELVONS) 

DOUBLE PRECISION DSEED 
COMMON/DOUBLE/ DSE ED 

COMM0N/TMRRST/NfTACRRX(3,l000)fTACRRY(3»lO00) t 
+TACRRZI 3,1000) 

COMMON/NOI SE/TACRMX< 1000) t TACRMYUOOO) .TACRMZI 1000) 
C0MMCN/PARAMT/G,G2,GK1,GK2 



SLNTRG=SGRT(TACRRX( 1,N)**2+TACRRY( 1,N)**2+ 
+TACRRZ( 1,N)**2) 

AZMITH=TRUHCG(TACRRX(1,N) tTACRRYI 1,N) ) 
GNDRNG=SGRTtTACRRXn,N)**2+TACRRY( 1,N )*=*2) 
SLVATN=ATAN(TACRRZ( 1 ,N)/GNDRNG) 
R=GGNQF(CSEED) 

RNGPNS=SLNTRG+R*RSI GMA 
A=GGNCF(OSEED) 

AZIPNS=AZMITH+A*ASIGMA 

E=GGNQF(DSEED) 

ELVPNS=ELVATN+E*ESIGMA 

TACRMX(N)=RNGPNS*COS( ELVPNS)*SIN( AZIPNS) 
TACRMY(N)=RNGPNS*COS(ELVPNS)*COS( AZIPNS) 
TACRMZ(N)=RNGPNS*SIN(ELVPNS) 

RETURN 

END 



SUBROUTINE AL P6TA ( CT , HDGERR , TTG ) 

C0MMCN/TMRRST/N,TACRRX(3, 1000),TACRRY(3,1000), 
+TACRRZ( 3,1000) 

COMMCN/N0ISE/TACRMX<1000J ,TACRMY( 1000) ,TACRMZ(i000) 
C0MMCN/FILTER/TACREX{3,1000) ,TACREY(3, 1000) , 
+TACREZ(3,1000) 

C0MMCN/PR0CES/X2SIGM,Y2SIGM,Z2SIGMtEWVRRX, EWVRRY, 
+GX ( 1000 ) ,G Y( 1000 ) ,GZ (1000) ,RAORES( 1000) , XRES ( 1000 ) , 
+YRES( 1000) ,ZR£S( 1000) , Wll( 100 ) , W 22( 1 000) , W33 ( 1000) , 

+P( 1000,9) ,PINITL 

0T2=DT*DT 

I«= (N .N£. 1) GG TO 10 

INITIALIZE FILTER. 

PP XKK=TACRMX( N) 

FPYKK=TACRMY(N) 

FPZKK=TACRNZ( N) 

FVXKK=TACRRX( 2,N) +£(^VRRX 
FVYKK=TACRRY(2,N) +EWVRRY 
FVZKK=TACRRZ(2,N) 

FOXKK1=FPXKK+OT*FVXKK 

F°YKK1=FPYKK+DT*FVYKK 

FPZKK1=FPZKK+0T*FVZKK 

strger=o.o 

strgrt=o.o 

MVRFLG=0 

MVRCNT=0.0 
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GO TO 9 <59 
10 CONTINUE 



20 

30 



40 

50 



SELECT PARAMETERS FOR FILTERING BASED ON 
MANEUVERING AND NOISE CRITERIA. 

HIGH OR LOW NOISE ENVIRONMENT? 

RUFRNG=SQRT<TACRMX(N)**2+TACRMY(NJ*^2+TACRMZI N)*«2) 
IF (RUFRNG .GT. 1500.) GO TO 20 
N0SFLG=0 
GO TO 30 
CONTINUE 
N0SFLG=1 
CONTINUE 

MANEUVERING? 

IF TTG .LT. 30 SEC ASSUME NO MANEUVERING AND GO ON. 

IF {TTG .LT. 3C.I GO TO 40 

CALL STRG( CT.NCSFLGt FDGERR » STRGER t STRGRT) 

CALL TMNVR(STRGRT,MVRCNT) 

IF ( <DT*FLOAT(MVRCNT) ) .GT. 5.0) MVRFLG=0 
IF { (CT*FLQAT(MVRCNT) ) .LT. -5.0) MVRFLG=0 
IF (ABS(HOGERR) .GT. .052359878) MVRFLG=1 
GO TO 50 
CONTINUE 
MVRFLG=0 
CONTINUE 

FIND ALFA/8ETA PARAMETERS . 

CALL CRSTRK(MGSFLG,MVRFLG,TTG,CALFAtC3ETA ) 

GX<N) =CALFA 



GY (N) =CALFA 
GZ(N)=CALFA 

FILTER X-CQORC. DATA. 
CURRENT STATES. 



XRESDU=TACRMX(N)-FPXKK1 

FPXKK=FPXKK1+CALFA*XRESDU 

fvxkk=fvxkk+cbeta*xresou/ot 

PREDICTED STATES. 

FPXKK1 = FPXKK+FVXKK=!>0T 

filter y-cooro. data. 

CURRENT STATES. 



YRES0U=TACRMY (N)-FPYKKl 

foykk=fpykki+calfa^yresdu 

FVYKK=FVYKK+C8ETA*YRESDU/DT 

predicted states. 

FPYKK1=FPYKK+FVYKK*DT 

FILTER Z-CCORC. DATA. 
CURRENT STATES. 
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c 

c 

c 

c 



999 



C 

C 

C 

C 



15 

20 



C 

C 

C 



10 

20 

30 



C 

C 

C 

C 



C 



ZRES0U=TACRMZ(NI-FPZKK1 

FPZKK=FPZKKH-CALFA*ZR£SOU 

FVZKK=FVZKK+CBETA*ZRESOU/OT 

PREDICTED STATES. 

FPZKK1=FPZKK+FVZKK*0T 

XR5S(N)=XRES0U 

YR£S<N)=YReS0U 

ZR5S(N)=ZR£S0U 

RADRES(N)=SQRT(XRES0U**2+YRESDU**2+ZRES0U’Mi2) 

CONTINUE 

TACREX( l,N)=FPXKK 
TACR5Y( 1»N)=FPYKK 
TACREZC 1,N)=FPZKK 
TACREX( 2»N)=FVXKK 
TACREYC 2tN) = FVYKK 
TACRcZt 2 fN) = FVZKK 
RETURN 
END 



SU3R0UTI NE STRG( DT »NOS FLGtHOGERR , STRGER, STRGRT ) 
IF (NOSFLG .EQ. 1) GO TO 15 
ALFA=.0625 
B£TA=.0C78125 
GO TO 20 
CONTINUE 

ALFA*. 02125 
BETA* .0C1953125 
CONTINUE 

STRGER=STRGER+ALFA* <HCG£RR-STRGER) 
STRGRT=STRGRT+BETA*(HDGERR-STRGER ) 
STRGER*STRGER+STRGRT*DT 
RETURN 

END 



SUBROUTINE TMNVR ( STRGRT » MVRCNT ) 

IF (A8S(STRGRTJ .GT. .00872665) GC TO 10 
IF (MVRCNT.lt. 0) MVRCNT=0 
MVRCNT=MVRCNT+1 
GO TO 30 

IF (ABS(STRGRT) .LT. .0174533) GO TO 20 
IF (MVRCNT .GT. 0) MVRCNT=C 
MVRCNT* MVRCNT-1 
GO TO 30 
MNRCNT=0 
CONTINUE 
RETURN 

END 



SUBROUTINE CR STRK ( NOSFL G»MVRFLG» TTG t C ALF A ,C86TA) 
IF (NCSFLG .EQ. 0) GO TO 50 
IF (MVRFLG .EQ. 0) GO TO 10 
LOW PRECISION (MANEUVERING) 

CALFA=.1875 
CBETA*. 0078125 
GO TC 30 
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20 



30 
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c 



60 



70 



80 

99 



C 

C 



C.' 



CONTINUE 

HIGH PRECISION < NGN-MANUEVERING) 

IF (TTG .LT. 30. )G0 TO 20 

MANUEVERING STOPPED WITH .GT. 30 SEC TO GO. 
CALFA=. 0390625 
CBETA=.0C02441406 
GO TO 30 
CONTINUE 

MANUEVERING STOPPED WITH .LT. 30. SEC TO GO, 
CALFA=. 078125 
C8ETA=.0C09765625 
GO TO 99 
LOW NCISE. 

CONTINUE 

IF {MVRFLG .£Q. 0) GO TO 60 
LOW PRECISION (MANEUVERING) 

CALFA=.375 
C8cTA=. 03125 
GO TO 80 
CONTINUE 

HIGH PRECISION (NON-MANEUVERING) 

IF (TTG .LT. 30.) GO TO 70 

MNVRNG STOPPED WITH 30 OR MORE SEC TTG. 
CALF A=.078125 
C8£TA=. 0009765625 
GO TO 80 
CONTINUE 

MNVRNG STOPPED WITH LESS THAN 30 SEC TTG. 
CALFA=. 15625 
CeSTA=.0039C625 
CONTINUE 
CONTINUE 
RETURN 



NO 



SUBRnuTTMF KALMMl ( KAD , OT ,RS IGMA , AS I GMA, E SIGMA , RANGE , 
+ AZMITH» ELEVTNtTTG) 



DOUBLE PRECISION CSEED 

COMMON/ DOUBLE /CS EEC 

COMMON/TMRRST/N,TACRRX( 3tl000) ,TACRRY(3, 1000) t 
+TACRPZ(3,1000) 

COMMON/NOISE/TACRMXdOOO), TACRMY(IOOO) ,TACRMZ( 1000) 

COMMON/ filter /TACREX (3, 1000 ) , TACREY ( 3 , 1000 ) , 
+TACREZ( 3.1000) 

CnMM0N/PR0CES/X2SIGM,Y2SIGM,Z2SIGMTEWVRRX,£WVRRY, 

+ GX(1000) ,GY( 1 COO) tGZ( 1000) ,RAOR£S( 1000) ,XRES( 1000) , 
+YPES( lOCC) ,ZR 5S( 1000) ,Wli( 1000) .W22( 1000) 

+ ,W33( lOCC) , 1000,9) ,PINITL 

DIMENSION 0(6,6) , PKK ( 6 ,6 ) , R ( 6 , 6) ,W(6,o),G(6,6), 

+PHI (6,6) ,0FL(6,6) ,DI (6,6).NULL(6,6 ) , H ( 6 , 6 ) , XKK ( 6 ) , 

+ XKKK6) , PK 1(6 ) .EXTRA 1(6. 6) , EXTR A2 ( 6 , o ) , PKK 1 ( 6 , 6 ) , 
+WKAREA( 13) , £XTRA3(5,3) ,£XTRA4(3,3) ,Z(6) 



IF (N .NE. 1 ) GO TO 25 
IDGT=2 

CT2=0T*0T/2.0 



DO 20 1=1,6 
Z( I )=C.O 
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c 

25 

C 



c 



Of? 10 J=l»6 
Q(I, J)=0.0 
PKK< I , J)=0.0 

IF (I .EQ. J) PKK(IfJ)= 
R(I, J)=0.0 
W( I » J)=0.0 
G(T,J)=0-0 
PHK I, J)=0.0 
DEU I,J)=0.0 
01 (ItJ)=0.0 

IF ( I .EQ. jj on I , J>=] 
NULLlIt J)=0.0 
H{ I, J)=0.0 
CONTINUE 
CONTINUE 

W1=X2SIGM*X2SIGM 

W2=Y2SIGM*Y2SIGM 

W3=Z2SIGM*Z2SIGM 

WX=0.0 

WY = WX 

V*Z=WY 

IXFLG=0 

IYFLG=0 

IZFLG=0 

DEL( 1»1)=0T2 

DEL(2,1)=0T 

DEU3,2) = 0T2 

DEL(4»2) =0T 

OEL(5»3)=OT2 

D5L{6t3)=DT 

H( 1,1 )=1.0 

H{ 2,3)=1.0 

H( 3,5)=1.0 

PHI (1,1) = 1.0 

PHKl ,2 ) =0T 

PH I (2, 2) =1.0 

PHI(3,3)=1.0 

PHI(3,4)=0T 

PHI(4,4) =1.0 

PHI(5,5 )=1.0 

PHI(5,6)=DT 

PH I (6, 6) =1.0 

RVAR=RSIGMA«RSIGf^A 

AVAR=ASIGMA*ASIGMA 

£VAR=ESIGMA*£SIGMA 

TACRSX(1,1)=TACRMX(1) 

XKK( 1 )=TACREX( 1,1 ) 

TACRSY(1,1)=TACR.MY(1) 

XKK(3)=TACRSY(1,1) 

TACREZ( 1,1)=TACRMZ< 1 ) 

XKK(5 )=TACREZ( 1,1) 

TACR5X ( 2 , 1 ) =T ACRRX ( 2 ,N ) +EWVRRX 
XKK( 2 ) = TACREX(2,1) 

TACR=Y( 2,1)=TACRRY(2,N)+EWVRRY 
XKK(4) =TACRSY (2, 1) 

TACRSZ( 2,l)=TACRRZ(2,N) 

XKK(6 )=TACREZ (2, 1) 



IF (N .EQ. 1) GO TO 99<3 



CONTINUE 

Z( l)=TACPMX(NJ 
Z( 2)=TACRMY(N ) 
Z( 3)=TACRMZ(N) 



PINITL 



.0 
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C 

C 



R2=RANGE*RANGE 

CA=COS(AZMITH J 

SA=SIN<AZMITH) 

CE=COS(ELEVTNJ 

SS=SIN(ELEVTN) 

CA2=CA*CA 

SA2=SA*SA 

CE2=CE*CE 

SE2=S£*SE 



R< 

R( 

R( 

R( 

R< 

R( 

R( 

R( 

R( 



1 , 1 )^ 
2,2 ) 

3.3) 
1 , 2 ) = 
2 , 1 ) = 

1.3) 
3,1) 

2.3) = 
3,2 ) = 



= R 2* ( £V A R’^ S E 2* SA 2 +A V A R*C E 2*C A 2 ) +R V A R*C E 2* S A 2 
=R2*(EVAR*SE2*CA2+AVAR*CE2*SA2 )+RVAR*C£2*CA2 
:R2*£VAR*CE2+RVAR*SE2 

= >?2*EVAR*SE2*SA*CA+(RVAR-R2*AVAR)*(CE2*SA=*CA) 
= R(1,2) 

= ( RVAR-R2*EVAR)*SE*CE*SA 
=R(1,3) 

:( RVAR-R2*EVAR)*SE*CE*CA 
= R(2,3) 



FKKl ) = 0.0 
FK 1(2)=0.0 
FK1(3)=C.0 
FKK A )=0.0 
FKK 5 )=Q.O 
FK 1(6) =0.0 



IF (KAD .N£. 0) GO TO 29 
W(l,l)=Wl 
W( 2, 2) = W2 
W(3,3)=W3 
GO TC 34 
29 CONTINUE 

IF (KAD .N£. 1) GO TO 31 

CALL A0PTV1(N,6,120, Wl, W2,W3,TACREX,TACREY,TACR£Z, 

+ W) 

GO TC 34 
31 CONTINUE 

IP (TTG .LT. 20.) GO TO 33 
XRS=XRES(N-1 ) 

YRS=YRES<N-1) 

ZRS=ZRES(N-1 ) 

CALL A0PTV2(6, XRS , IXFLG, YRS , I Y FLG, ZRS , I ZFLG, PKKJ 

33 CONTINUE 
W(l, 1)=W1 
W( 2, 2)=W2 
W( 3, 3) = W3 

34 CONTINUE 
W11(N) = W(1 ,1) 

W22(N)=H(2,2) 

W33(N)=W(3,3) 

GENERATE Q-MATRIX. 

CALL 0UADPS(D£L,W,NULL,6,6,Q) 



X( K/K-1 )=PHI (K,K-1 J*X(K-1/K-1) +F(K-1 ) 

CO 210 1=1,6 

XKK1(I)=FK1(I > 

DO 200 J=l,6 

XKKKI ) =XKK1( I ) + PHI(I ,J)*XKK( J) 
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200 CONTINUE 
210 CONTINUE 



P( K/K-1 )=PH I (K/K-1)*P< K-l/K-1) ♦TRANSPOSE ( PHI ) +Q( K-ll 
CALL QUA0PS(PHI,PKK,Q,6t6,PKKU 

G( K) = P(K/K-1 )*H(K )*INV(H(K )*P{K/K-1)*TRANSP0SE(H) +R(K) ) 
CALL gUACPS(H,PKKl,R,6,6,EXTRAl ) 

DO 212 I =1,3 
00 211 J=l,3 

EXTRA4( I,J)=£XTRAUI,J) 

211 CONTINUE 

212 CONTINUE 

00 230 1=1,6 
00 225 J=l,6 

EXTRA2( I,J)=0.0 
DC 220 K=l,6 

EXTRA2( I , J)=EXTRA2( I, J )+PKKl(I,KJ*H< J,K) 
CONTINUE 



C 

C 

C 

C 



220 

225 

230 



CONTINUE 

CONTINUE 



INVERT EXTRAl AND MULTIPLY BY EXTRA2. 

CALL LINV2F(EXTRA4,3,3,EXTRA3, IDGT,WKAREA,IER) 
CALL PRCDCT(EXTRA2,EXTRA3,3,3,6,G) 

GX (N)=G( 1, 1) 

GY(N)=G<3,2) 

GZ(N)=G(5,3) 

C X( K/K)=X(K/X-l)+G(K)*(Z(K)-h(K)*X(K/K-l) ) 

DO 250 1=1,6 

5XTRAK I ,1 )=Zm 
00 240 J =1,6 

EXTRAK I,1)=6XTRA1(I,1)-H< I,J)*XKK1(J) 
240 CONTINUE 

250 continue 

RADRES( N)=SQPT( EXTRAK 1, 1J4^2+EXTRA1 ( 2, 1 )^*2+ 

+ EX7RAU 3,1)*«2) 

XRES(N)=EXTRAi(l,li 
YRES(Ni = EXTRAi(2,U 
ZR£S(N)=EXTRA1(3,1J 
DO 270 1=1,6 

XKKd )=XKKim 
DO 26C J=l,3 

XKK( I )=XKK{ I )+G< I,J)*EXTRA1( J , 1) 

260 CONTINUE 

270 CONTINUE 

TACREXI 1,N)=XKK( 1) 

TACREX( 2,N)=XKK(2J 
TACREY( 1,N)=XKK( 3) 

TACREY( 2,NJ=XKKI4) 

TACREZ( 1 ,N)=XKK( 51 
TACR6Z(2,N)=XKK<6) 

C 

C P( K/K)=?( K/K-l)-G(K)*H(K)*P(K/K-i) 

CALL PRC0CT(G,h,6,6,6,eXTRAl) 

CALL PRCDCT<EXTRA1,PKK1 ,6,6,6, EXTRA2 ) 

DO 290 1=1,6 
DO 280 J=l,6 

230 

290 CONTINUE 



999 CONTINUE 
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c 



c 

c 

c 



c 

c 

c 



c 



c 

c 

c 



c 

c 



10 

20 



Pi N,l) = 
P(N,2) = 
P(N,3) = 
o(N,4) = 
P(N,5) = 
P( N,6)= 

RETURN 

END 



:PKK(lt li 
■PKK(2 t 2) 
=PKK(3t3) 
:PKK(4» 4) 
= PKK(5,5) 
PKK(6»6) 



SUBROUTINE KALMN2< KAD t DT »RS IGMA , ASI GMA» ES IGMA , RANGE t 
■►AZMITH, 5LEVTN,TTGJ 



DOUBLE PRECISION OSEED 

COMMON/DCUBLE/DSEED 

CQMM0N/TMRRST/N,TACRRX<3f 1000) »TACRRY(3»1000) , 
+TACRRZ<3»1000) 

COMMON/NCISE/TACRMXI 1000) , TACRMY (lOOOJ , TACRMZ ( 1000 ) 

COMMON/FILTER/TACREX (3, 1000 J ,TACR£Y< 3, 1000) » 
♦TACREZ( 3.1000) 

COMMON/ PR0CES/X2S I GM,Y2SIGM,Z2SIGM,EWVRRX»EWVRRY, 
+GX(1000) tGY(lOOO) fGZ( 1000), RADRES( 1000) »XRES( 1000) , 
♦ YRES( 1000) ,ZP ESI 1000 ). W1 1 ( 1000) ,W22( 1000) 

+,W33( 1000) ,P( 1000,9) .PINITL 

DIMENSION Q(9,9) , PKK (9 , 9 ) , R( 9 , 9 j ,W( 9 ,9) ,G(9,9 ) , 

+PH 1(9, 9) .DEL (9, 9) , 01 ( 9 , 9 ) , NULL ( 9 ,9 ) , H( 9 , 9 ) , XKK ( 9 ) , 

+ XKKK 9) ,FK1(9 j ,EXTRA1(9,9) , EXTR A2 ( 9 , 9 ) , PK.K1 ( 9 , 9 ) 
+,WKAPSA(75) ,EXTRA3(3,3) ,EXTRA4( 3,3) ,Z(9) 



IF (N .NE. 1 ) GO TO 25 
IDGT=2 

DT2=DT*0T/2.0 

DT3=DT2*CT/3.0 



DO 20 1=1,9 
Z( I )=0.0 
DO 10 J=l,9 
Q (I, J) = C.O 
PKK( I , J)=0.0 
IF (I .EG. 
R( I,J) = 0.0 
W< I, J)=C.O 
G(I, J)=C.O 
PHK I , J )=0.0 
0EL( I , J )=0.0 
DI (I , J)=0.0 
IF (I .EG. 
NULLd , J)=0.0 
H( I, J)=0.0 
CONTINUE 
CONTINUE 

VI1=X2SIGM*X2SIGM 

W2 = Y2SIGM=<‘Y2SIGM 

W3=Z2SIGM’>Z2SIGM 

WX=0.0 

WY=WX 

WZ=WY 

IXFLG=0 

IYFLG=0 



J) 



J) 



PKK( I ,J)=PINITL 



CI( I , J) = l .0 
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IZFLG=0 

0EL(1,1)=0T3 

0EL(2,1 )=DT2 

0EL(3,1)=DT 

CEL(4,2)=0T3 

0EL(5,2)=0T2 

DEL( 6,2|=0T 

DEL(7,3)=DT3 

DEL(8»3)=DT2 

DEL(9,3)=0T 

H < 1,1 ) = 1.0 

H( 2,4)=1.0 

H( 3»7) = 1.0 

PHKl ,1)=1.0 

PHI(1,2)=DT 

PHK 1,3 >=0T2 

PHI(2,2)=1.0 

PHK 2,3)=0T 

PH 1(3, 3) = 1.0 

PH I (4, 4) =1.0 

PHI(4,5)=0T 

PH1(4,6 )=0T2 

PH I (5, 5) =1.0 

PH1(5,6)=DT 

PHI(6,6)=1.0 

PHI(7,7) =1.0 

PHI(7,8)=DT 

PHI(7,9) =DT2 

PHK 8,8) =1.0 

PHI(8,9 )=0T 

PHI(9,9) =1.0 

PVAR=RS1GMA*R SIGMA 

AVAR=ASIGMA*ASIGMA 

EVAR=ESIGMA=*'ESIGMA 

TACREX(1,N)=TACRMX(1 ) 

XKK( 1 )=TACREX (1,N) 

TACREX( 2,N)=TACRRX(2,NJ+EWVRRX 
XKK(2 )=TACREX (2,N) 
XKK(3)=TACRRZ(3,N) 
TACR£Y(1,N)=TACRMY(1) 
XKK(4)=TACREY(1,N) 

TACREYC 2,N)=TACRRY(2 ,N) +EWVRRY 
XKK( 5 )=TACREY(2,N) 
XKK(6)=TACRRY(3,N) 

TACREZ( 1,N)=TACRMZ(1 ) 
XKK(7)=TACREZ(1,N) 

TACREZ( 2,N)=TACRRZ(2,NJ 
XKK(8)=TACREZ(2,N) 

XKK( 9)=TACRRZ (3,N J 

IF (N .SO. 1) GO TO 999 



25 



CONTINUE 



C 

c 



Z( 1)=TACRMX(N ) 
Z( 2)=TACRMY(N) 
Z( 3>=TACRMZ(N ) 



R2=RANGE*RANGE 
CA=CCS( AZMITH) 
SA=SIN( AZMITH) 
CE=CPS{ HLEVTN) 
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SE=SIN(EL£VTN) 

CA2=CA*CA 

SA2=SA*SA 

CE2=C£*CE 

SE2=SE*S5 



R( i, l) = R2*(EVAf<*SE2*SA2+AVAR*CE2*CA2)+RVAR*CE2^SA2 
R( 2»2)=R2*(EVAR*SE2*CA2+AVAR*CE2*SA2}+RVAR*C£2*CA2 
R( 3,3)=R2*EVAR*CE2+RVAR*SE2 

R( If 2)= R2*EVAR»S£2*SA*CA+( RVAR-R2*AVAR)=»‘| CE2*SA*CA j 
R(2»l) = Ra,2) 

R( l,3)=(RyAR-R2*EVAR)*SE*CE*SA 
R(3fl)=R(1.3) 

R( 2f 3)=(RVAR-P2*EVAR)*SE*CE*CA 
R(3,2)=R (2,3) 



FKKl )=C.O 
FK1(2)=0.0 
FK1(3)=0.0 
FK1(4)=0.0 
FK1(5)=0.0 
FK116)=0,0 
FK1(7)=0.0 
FK1(8)= 0.0 
FKl(9)=0.0 



IF (KAD .NE. 0) GO TO 29 
W( 1, 1)=W1 
W( 2 , 2 )=W 2 
W(3,3)=W3 
GO TO 34 
29 CONTINUE 

IF (KAD .NE. 1) GO TO 31 

CALL ADPTVl(Nf9,120f Wl,k2fW3f TACRSXfTACREY, TACREZ, 

+ W) 

GO TO 34 
31 CONTINUE 

IF (TTG .LT. 20.) GO TO 33 
XRS=XR£S(N-1) 

YRS=YPES(N-1 ) 

ZPS=ZRES(N-1) 

CALL A0PTV2(9,XRS,IXFLG,YRS,IYFLG, ZRS , I ZFLG, PKK) 

33 CONTINUE 
W< 1, 1)=W1 
W( 2, 2)=W2 
W(3, 3)=W3 

34 continue 
W11(N)=W(1,1) 

W 22 (N)=W( 2 , 2 ) 

W33(N)=W( 3,3) 

GENERATE 0-MATRIX. 

CALL QUA0PS(0EL,W,NULL,9,9,Q) 



X( K/K-1) = PH I (K ,K-1 )*X( K-l/K-1) +F(K-1 ) 

DO 210 1=1,9 

XKKK IJ=FK1(I ) 

DO 200 J=l,9 

XKKK I )=XKK1(1 )+PHI( I, J)*XKK( J ) 
200 CONTINUE 

210 CONTINUE 
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P( K/K-II = PHI (K/K- 1 )*P(K-1/K-1) ♦TRANS POSE (PHI )+Q(K-lJ 
CALL QUA0PS(PHI,PKK,Q,9,9,PKK1) 



G( K)=P(K/K-1)^H(K)*INV(H(K)^P(K/K-1 J^TRANSPOSE(H)+R(K ) ) 
CALL aUADPS(H,PKKl,R,9, 9,£XTRA1) 

00 212 I =1,3 
00 211 J=l,3 

EXTRA4( I,J J=EXTRA1(I,J) 

211 CONTINUE 

212 CONTINUE 

00 230 1=1,9 
00 225 J=l,9 

EXTRA2( I,J)=0.0 
00 220 K=l,9 

EXTRA2( I, J)=5XTRA2(I, J)+PKK1(I, K1#H( J,K) 
220 CONTINUE 

225 CONTINUE 

230 CONTINUE 



INVERT EXTRAl ANO MULTIPLY BY EXTRA2. 

CALL LI NV2F(EXTRA4,3,3,EXTRA3, I DGT, WKAREA, IER ) 
CALL PRC0CT(EXTRA2,EXTRA3,3,3,9,G) 

GX(N)=G(1,1) 

GY (N)=G(A,2) 

GZ(N)=G(7,3I 

X(K/KJ =X(K/K-1)+G(K)^(Z(KJ-H(KJ*X(K/K-1J ) * 

DO 250 1=1,9 

EXTRAK I,1) = Z(I ) 

DO 2^0 J =1,9 

EXTRAK I ,1J=£XTRA1(I , 1 J-ri ( I , J I ♦XKKi ( J ) 
240 CONTINUE 

250 CONTINUE 

RA0RSS( N)=SQRT( EXTRAK 1 , 1)**2+EXTRA1( 2, 1 )**2+ 
+EXTRA1(3,1)^^2) 

XRES(N) = EXTRAK1,1 J 
YRES(N) = EXTRAK2,1) 

ZRES(N)=EXTRA1(3, 1} 

DO 270 1=1,9 

XKK( I )=XKKK I ) 

DO 260 J=l,3 

XKKm=XKK( II+G(I , JJ ♦EXTRA KJ,1) 

260 CONTINUE 

270 CONTINUE 

TACREXI 1,N)=XKK( 1) 

TACREX(2,N)=XKK(2) 

TACREX( 3,N)=XKK( 3) 

TACREYt 1,N)=XKK(4) 

TACREY( 2,N)=XKK(5I 
TACREY( 3,NJ=XKK( 6) 

TACREZI 1,N)=XKK(7) 

TACREZ(2,N)=XKK(8J 
TACREZ( 2,N J=XKK( 9) 



P( K/K)=P(K/K-1 )-G (K)^H(KJ^P(K/K-1J 
CALL PRGCCT(G,H,9,9,9, EXTRAl J 
CALL PR0CCT(EXTRA1 ,PKK1 ,9,9,9, SXTRA2 I 
DO 290 1=1,9 
DO 28C J=l,9 

PKK(I, J)=PKK1( I, JJ-EXTRA2(I,J) 
230 CONTINUE 

290 CONTINUE 
999 CONTINUE 
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c 

C15 



c 

c 

c 



100 

200 

300 

400 



C 

c 

c 



27 

35 



37 

41 



C 

C 

C 



P(N,1)=PKK<1»1) 

P(N,2)=PKK(2, 2) 

P(N»3)=PKK(3,3) 

P( Nt4)=PKK(4,4) 

P(N,5 ) = PKK(5» 5) 

P( N,6)=PKK(6,6) 

P(N,7)=PKK(7, 7) 

P(N,8)=PKK<8, 8) 

P<N»9)=PKK(9»’;) 

FORMAT ( • • t »G( 't lit* »* » II* * ) = * ,F20.10) 

RETURN 

END 



SUBROUTINE QUAOPS ( X , H» R » M, N , C ) 

DIMENSION X<M,N),H(N*N)*R(M,M),C(MtMJ 
DO 400 I =ltM 
DO 300 J=ltM 
C( I, J)=R(It J) 

DC 200 K=1,N 
DO IOC L=ltN 

C( I, JJ=C( I,J)+X( I,K)#H(K,L)*X(J,U 
CONTINUE 
CONTINUE 
CONTINUE 
CONTINUE 
RETURN 

END 



SUBROUTINE ADPTVKN* 13 ,JSTARTtWl , W2 » W3, T ACP5X » TACREY , 
+TACREZtW) 

01 MENSIGN TACREX( 3,1000) , TACREYI 3 , 1000 ) , 

+TACRS2( 3,1000 ),W( 12, 13) 

ISTATE=I3/3 

IF (N .LT, JSTART) GO TO 27 

WX=8.0*(TACR£X(ISTAT£,N-1 )-TACR£X( ISTAT£,N-2) ) 
WY=a. C*<TACR£Y(ISTATE,N-l)-TACR£Y( ISTATE,N-2) ) 
WZ=8.C*<TACR£ZUSTAT£,N-l)-TACR£2(ISTAT£,N-2)) 
GC TO 35 
CONTINUE 
WX=0.0 
WY=0.0 
WZ=C.O 
CONTINUE 

IF ( TSTATE .£Q. 3) GC TO 37 
W(l,l)=Wl+(WX*WX)/3. 

W(2,2)=W2+(WY*WY)/3. 

W(3,3)=W3+(WZ*WZ)/3. 

GO TC 41 
CONT INUE 

W( 1, l) = Wl+( WX^WX) /81. 

W(2,2» = V«2'MWY^WY)/81. 

W (3,3)=W3+(WZ*WZ)/8i. 

CONTINUE 

RETURN 

END 



SUBROUTINE ADFTV2 (NORDER,XR£S, IXFLG,YRES, IYFLG,ZR£S, 
+IZ=LG,PKK) 

DIMENSION PKK(NCROER,NOROER) 

RESET=1000. 
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10 



12 

14 



16 

20 



C 

c 



10 

12 

1 ^ 

20 

30 

32 

40 



8) GO TO 14 
GO TO 12 



END 



CALL 8IAS(XRES,IXFLG) 

IF (lABSdXFLGi .LT. 8) GO TO 10 
PKK(1,1)=RESET 
PKK<2,2)=RES£T 

IF (NOROER .EQ. 9) PKK( 3 , 3 )=RE SET 
CONTINUE 

CALL 8IAS( YRES.IYFLGJ 
IF UABS(IYFLG) .LT. 

IF (NORCER .EQ. 9) 

PKK(3,3)=RESET 
PKK(4,4)=RESET 
GO TO 14 
CONTINUE 
PKK(4t4)=RESET 
PKK( 5,5)=RESET 
PKK(6»6)=RESET 
CONTINUE 

CALL BIAS( ZRES;UFLG) 

IF (lABS(IZFLG) .LT. 8J GO TO 20 
IF (NOROER .EQ. 9) GO TO 16 
PKK( 5,5)=RESET 
PKK(6,6)=RESET 
GO TG 20 
CONTINUE 
PKK<7,7 i=P.ESET 
PKK< 8,8)=RESET 
PKK{9»9 )=RES£T 
CONTINUE 
RETURN 



SUBROUTINE BI A S( RESI DU , I FLAG ) 

IF (RESIDU) 10,20»30 

IF (IFLAG .LE. 0) GO TO 12 
IFLAG=0 
GO TG 40 
CONTINUE 

IFLAG=IFLAG-1 
GC TO 40 
IFLAG=0 
GO TO 40 

IF (IFLAG .GE. Oi GO TO 32 
IFLAG=0 
GO TC 40 
CONTINUE 
IFLAG=I FLAG+1 
CONT INUE 
RETURN 

END 
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